// This code is in the public domain. See LICENSE for details.

// math3d implementation

#include "types.h"
#include "math3d_2.h"

namespace fr
{
	static void frSinCos(sF32 parm, sF32* sinv, sF32* cosv)
	{
		__asm
		{
			mov				eax, [cosv];
			mov				ebx, [sinv];
			fld				dword ptr [parm];
			fsincos;
			fstp			dword ptr [eax];
			fstp			dword ptr [ebx];
		}
	}

  // ---- vector2

  void vector2::addscale(const vector2& a, const sF32 f)
  {
    x += a.x * f;
    y += a.y * f;
  }

  void vector2::addscale(const vector2& a, const vector2& b, const sF32 f)
  {
    x = a.x + b.x * f;
    y = a.y + b.y * f;
  }

  sF32 vector2::dot(const vector2& a) const
  {
    return x * a.x + y * a.y;
  }

  void vector2::lerp(const vector2& s1, const vector2& s2, sF32 t)
  {
    x=s1.x + (s2.x - s1.x) * t;
    y=s1.y + (s2.y - s1.y) * t;
  }

  sF32 vector2::distanceToSquared(const vector2& v2) const
  {
    return (x - v2.x) * (x - v2.x) + (y - v2.y) * (y - v2.y);
  }

  // ---- vector

  vector vector::operator * (const matrix& m) const
  {
    vector ret = *this;
    ret.transform(m);

    return ret;
  }

  void vector::addscale(const vector& a, const sF32 f)
  {
    x += a.x * f;
    y += a.y * f;
    z += a.z * f;
  }

  void vector::addscale(const vector& a, const vector& b, const sF32 f)
  {
    x = a.x + b.x * f;
    y = a.y + b.y * f;
    z = a.z + b.z * f;
  }

  void vector::transform(const vector& src, const matrix& m)
  {
    v[0]=src.v[0]*m(0,0)+src.v[1]*m(1,0)+src.v[2]*m(2,0)+m(3,0);
    v[1]=src.v[0]*m(0,1)+src.v[1]*m(1,1)+src.v[2]*m(2,1)+m(3,1);
    v[2]=src.v[0]*m(0,2)+src.v[1]*m(1,2)+src.v[2]*m(2,2)+m(3,2);
  }

  void vector::transform(const matrix& m)
  {
    vector ret=*this;
    transform(ret, m);
  }

  void vector::rotate(const vector& src, const matrix& m)
  {
    v[0]=src.v[0]*m(0,0)+src.v[1]*m(1,0)+src.v[2]*m(2,0);
    v[1]=src.v[0]*m(0,1)+src.v[1]*m(1,1)+src.v[2]*m(2,1);
    v[2]=src.v[0]*m(0,2)+src.v[1]*m(1,2)+src.v[2]*m(2,2);
  }

  void vector::rotate(const matrix& m)
  {
    vector ret = *this;
    rotate(ret, m);
  }

  sF32 vector::dot(const vector& a) const
  {
    return x*a.x+y*a.y+z*a.z;
  }

  void vector::cross(const vector& a, const vector& b)
  {
    x=a.y*b.z-a.z*b.y;
    y=a.z*b.x-a.x*b.z;
    z=a.x*b.y-a.y*b.x;
  }

  void vector::lerp(const vector& s1, const vector& s2, sF32 t)
  {
    x=s1.x+(s2.x-s1.x)*t;
    y=s1.y+(s2.y-s1.y)*t;
    z=s1.z+(s2.z-s1.z)*t;
  }

  sF32 vector::distanceToSquared(const vector& v2) const
  {
    return (x-v2.x)*(x-v2.x)+(y-v2.y)*(y-v2.y)+(z-v2.z)*(z-v2.z);
  }

  vector& vector::operator *= (const matrix& m)
  {
    transform(m);
    return *this;
  }

  // ---- matrix

  void matrix::invertTranslateVia(const matrix& in)
  {
    (*this)(3,0)=-((*this)(0,0)*in(3,0) + (*this)(1,0)*in(3,1) + (*this)(2,0)*in(3,2));
    (*this)(3,1)=-((*this)(0,1)*in(3,0) + (*this)(1,1)*in(3,1) + (*this)(2,1)*in(3,2));
    (*this)(3,2)=-((*this)(0,2)*in(3,0) + (*this)(1,2)*in(3,1) + (*this)(2,2)*in(3,2));
    (*this)(3,3)=1.0f;
  }

  void matrix::mul(const matrix& a, const matrix& b)
  {
    for (sInt i=0; i<4; i++)
    {
      for (sInt j=0; j<4; j++)
      {
        (*this)(i,j)=a(i,0)*b(0,j)+a(i,1)*b(1,j)+a(i,2)*b(2,j)+a(i,3)*b(3,j);
      }
    }
  }

  void matrix::inverseAP(const matrix& in)
  {
	  sF32 scale=1.0f/(in(0,0)*in(0,0)+in(0,1)*in(0,1)+in(0,2)*in(0,2));

	  (*this)(0,0)=scale*in(0,0); (*this)(0,1)=scale*in(1,0); (*this)(0,2)=scale*in(2,0); (*this)(0,3)=0;
	  (*this)(1,0)=scale*in(0,1); (*this)(1,1)=scale*in(1,1); (*this)(1,2)=scale*in(2,1); (*this)(1,3)=0;
	  (*this)(2,0)=scale*in(0,2); (*this)(2,1)=scale*in(1,2); (*this)(2,2)=scale*in(2,2); (*this)(2,3)=0;

    invertTranslateVia(in);
  }

  void matrix::inverse(const matrix& in)
	{
	  sF32 scale=1.0f/(in(0,0)*in(1,1)*in(2,2)+in(1,0)*in(2,1)*in(0,2)+in(2,0)*in(0,1)*in(1,2)-
	                    in(0,0)*in(2,1)*in(1,2)-in(1,0)*in(0,1)*in(2,2)-in(2,0)*in(1,1)*in(0,2));

	  (*this)(0,0)=scale*(in(1,1)*in(2,2) - in(1,2)*in(2,1));
	  (*this)(0,1)=scale*(in(2,1)*in(0,2) - in(0,1)*in(2,2));
	  (*this)(0,2)=scale*(in(0,1)*in(1,2) - in(1,1)*in(0,2));
	  (*this)(0,3)=0;
	  (*this)(1,0)=scale*(in(2,0)*in(1,2) - in(1,0)*in(2,2));
	  (*this)(1,1)=scale*(in(0,0)*in(2,2) - in(0,2)*in(2,0));
	  (*this)(1,2)=scale*(in(1,0)*in(0,2) - in(0,0)*in(1,2));
	  (*this)(1,3)=0;
	  (*this)(2,0)=scale*(in(1,0)*in(2,1) - in(2,0)*in(1,1));
	  (*this)(2,1)=scale*(in(2,0)*in(0,1) - in(0,0)*in(2,1));
	  (*this)(2,2)=scale*(in(0,0)*in(1,1) - in(1,0)*in(0,1));
	  (*this)(2,3)=0;

    invertTranslateVia(in);
	}

  void matrix::transpose(const matrix& in)
  {
    for (sInt i=0; i<4; i++)
    {
      for (sInt j=0; j<4; j++)
        (*this)(i,j)=in(j,i);
    }
  }

  void matrix::invTrans3x3(const matrix& in)
  {
    identity();
    
    sF32 scale=1.0f/(in(0,0)*in(1,1)*in(2,2)+in(1,0)*in(2,1)*in(0,2)+in(2,0)*in(0,1)*in(1,2)-
      in(0,0)*in(2,1)*in(1,2)-in(1,0)*in(0,1)*in(2,2)-in(2,0)*in(1,1)*in(0,2));
    
    (*this)(0,0)=scale*(in(1,1)*in(2,2) - in(1,2)*in(2,1));
    (*this)(0,1)=scale*(in(2,0)*in(1,2) - in(1,0)*in(2,2));
    (*this)(0,2)=scale*(in(1,0)*in(2,1) - in(2,0)*in(1,1));
    (*this)(1,0)=scale*(in(2,1)*in(0,2) - in(0,1)*in(2,2));
    (*this)(1,1)=scale*(in(0,0)*in(2,2) - in(0,2)*in(2,0));
    (*this)(1,2)=scale*(in(2,0)*in(0,1) - in(0,0)*in(2,1));
    (*this)(2,0)=scale*(in(0,1)*in(1,2) - in(1,1)*in(0,2));
    (*this)(2,1)=scale*(in(1,0)*in(0,2) - in(0,0)*in(1,2));
    (*this)(2,2)=scale*(in(0,0)*in(1,1) - in(1,0)*in(0,1));
  }

	sF32 matrix::det3x3() const
	{
		const matrix& m = *this;

		return m(0,0) * m(1,1) * m(2,2) + m(0,1) * m(1,2) * m(2,0) + m(0,2) * m(1,0) * m(2,1)
			   - m(2,0) * m(1,1) * m(0,2) - m(2,1) * m(1,2) * m(0,0) - m(2,2) * m(1,0) * m(0,1);
	}

  void matrix::identity()
  {
    memset(_v, 0, sizeof(sF32)*4*4);
    (*this)(0,0)=1.0f;
    (*this)(1,1)=1.0f;
    (*this)(2,2)=1.0f;
    (*this)(3,3)=1.0f;
  }

  void matrix::rotateHPB(sF32 hd, sF32 pt, sF32 bk)
	{
		sF32 sh, ch, sp, cp, sb, cb;
		frSinCos(hd, &sh, &ch);
		frSinCos(pt, &sp, &cp);
		frSinCos(bk, &sb, &cb);

	  (*this)(0,0)=ch*cb+sb*sp*sh;
	  (*this)(0,1)=ch*sb-sh*sp*cb;
	  (*this)(0,2)=sh*cp;
		(*this)(0,3)=0;
	  (*this)(1,0)=-sb*cp;
	  (*this)(1,1)=cp*cb;
	  (*this)(1,2)=sp;
		(*this)(1,3)=0;
	  (*this)(2,0)=ch*sp*sb-sh*cb;
	  (*this)(2,1)=-ch*sp*cb-sh*sb;
	  (*this)(2,2)=ch*cp;
		(*this)(2,3)=0;
		(*this)(3,0)=0;
		(*this)(3,1)=0;
		(*this)(3,2)=0;
		(*this)(3,3)=1;
	}

  void matrix::camera(const vector& pos, const vector& lookAt, const vector& up)
	{
		vector fwd = lookAt - pos;
	  fwd.norm();

    vector upt = up;
    upt.norm();
    if (fwd.distanceToSquared(upt)<1e-6f)
      upt.z+=1e-7f;

    vector upp = upt - (fwd * (upt * fwd));
	  upp.norm();

    vector rgt=upp%fwd;

	  (*this)(0,0)=rgt.x;     (*this)(0,1)=upp.x;     (*this)(0,2)=fwd.x;     (*this)(0,3)=(sF32) 0.0f;
	  (*this)(1,0)=rgt.y;     (*this)(1,1)=upp.y;     (*this)(1,2)=fwd.y;     (*this)(1,3)=(sF32) 0.0f;
	  (*this)(2,0)=rgt.z;     (*this)(2,1)=upp.z;     (*this)(2,2)=fwd.z;     (*this)(2,3)=(sF32) 0.0f;
	  (*this)(3,0)=-pos*rgt;  (*this)(3,1)=-pos*upp;  (*this)(3,2)=-pos*fwd;  (*this)(3,3)=(sF32) 1.0f;
	}
}
